#ifndef STATECLIMB_H
#define STATECLIMB_H

#include "state.h"
#include "stateid.h"
#include "../eventmanager.h"
#include "../parammanager.h"
#include "../route.h"
#include "../properties.h"

namespace state {

const uint32_t ACCELERATE_TIME = 15 * 1000;
const uint32_t FLAPSRETRACT_TIME = 1 * 1000;
	
struct ClimbInitial : public State {
	ClimbInitial(int id) : State(id) {}
	
	virtual void onEnter() {
		::Route *route = Manager::Instance().getRoute();
		
		if (!route->isOnPattern())
			route->flyTo(0, false);	// don't override oper

		ParamManager *parammgr = Manager::Instance().getParamManager();

		PropAccessor p;
		p().fcs.roll.set(Roll_SetPoint, 0);
		p().fcs.pitch.set(Pitch_SetPoint, parammgr->getFloat(ParamManager::PITCH_CLIMB));	// constant pitch climb
//		p().fcs.pitch.set(Pitch_AirSpeed, parammgr->getFloat(ParamManager::VCLIMB) / 3.6f); // constant airspeed climb (not used)
		p().fcs.flaps.set(Flaps_Value, Flaps_Value_Takeoff);

		p().fcs.state = getId();
	}
};


struct ClimbAccelerate : public State {
	ClimbAccelerate(int id) : State(id) {}	
	
	virtual void onEnter() {
		Manager::Instance().getEventManager()->setTimer(ACCELERATE_TIME);

		PropAccessor p;
		p().fcs.roll.set(Roll_Track, 15);
		p().fcs.pitch.set(Pitch_Altitude_Baro, p().air_data.altitude_est + 20.0f);
		p().fcs.flaps.set(Flaps_Value, Flaps_Value_Takeoff);

		p().fcs.state = getId();
	}
};


struct ClimbRetractFlaps : public State {
	ClimbRetractFlaps(int id) : State(id) {}
	
	virtual void onEnter() {
		Manager::Instance().getEventManager()->setTimer(FLAPSRETRACT_TIME);

		PropAccessor p;
		p().fcs.roll.set(Roll_Track, 15);
		p().fcs.flaps.set(Flaps_Value, Flaps_Value_Retracted);

		p().fcs.state = getId();
	}
};


struct Climb : public State {
	enum {NORMAL = false, MISSED_APPROACH = true};

	Climb(bool missedApproach) : State(), missedApproach_(missedApproach) {
		State *initial = new ClimbInitial(stateid::CLIMB_INITIAL);
		State *accelerate = new ClimbAccelerate(stateid::CLIMB_ACCELERATE);
		State *retractFlaps = new ClimbRetractFlaps(stateid::CLIMB_RETRACT);

		setInitial(initial);
		initial->addTransition(evt_initial_alt, accelerate);
		accelerate->addTransition(evt_vretract, retractFlaps);
		accelerate->addTransition(evt_timer, retractFlaps);
		retractFlaps->addTransition(evt_timer, NULL);
	}

	virtual void onEnter() {
		if (missedApproach_)
			Manager::Instance().getRoute()->flyToPattern();

		PropAccessor p;

		p().fcs.yaw.set(Yaw_Damper);
		p().fcs.throttle.set(Throttle_Value, 1.0f, 10);
		p().fcs.brakes.set(Brakes_Value, 0);

		p().fcs.watcher = Watcher_Idle;
		
		p().fcs.parachute_armed = false;		
	}

private:
	Climb();

	bool missedApproach_;
};

}

#endif
